/**
    @author: dt
    @date: 2025-01-23
    @description: 定点数3D向量实现
*/

using System;
using UnityEngine;

namespace DLib.FPMath
{
    /// <summary>
    /// 定点数3D向量
    /// </summary>
    [Serializable]
    public struct FPVec3 : IEquatable<FPVec3>
    {
        public FPNumber x;
        public FPNumber y;
        public FPNumber z;

        #region 构造函数

        public FPVec3(FPNumber x, FPNumber y, FPNumber z)
        {
            this.x = x;
            this.y = y;
            this.z = z;
        }

        public FPVec3(FPNumber value)
        {
            this.x = value;
            this.y = value;
            this.z = value;
        }

        public FPVec3(FPVec2 xy, FPNumber z)
        {
            this.x = xy.x;
            this.y = xy.y;
            this.z = z;
        }

        public FPVec3(Vector3 vector)
        {
            this.x = new FPNumber(vector.x);
            this.y = new FPNumber(vector.y);
            this.z = new FPNumber(vector.z);
        }

        #endregion

        #region 静态常量

        public static FPVec3 Zero => new FPVec3(FPNumber.Zero, FPNumber.Zero, FPNumber.Zero);
        public static FPVec3 One => new FPVec3(FPNumber.One, FPNumber.One, FPNumber.One);
        public static FPVec3 Up => new FPVec3(FPNumber.Zero, FPNumber.One, FPNumber.Zero);
        public static FPVec3 Down => new FPVec3(FPNumber.Zero, -FPNumber.One, FPNumber.Zero);
        public static FPVec3 Left => new FPVec3(-FPNumber.One, FPNumber.Zero, FPNumber.Zero);
        public static FPVec3 Right => new FPVec3(FPNumber.One, FPNumber.Zero, FPNumber.Zero);
        public static FPVec3 Forward => new FPVec3(FPNumber.Zero, FPNumber.Zero, FPNumber.One);
        public static FPVec3 Back => new FPVec3(FPNumber.Zero, FPNumber.Zero, -FPNumber.One);

        #endregion

        #region 属性

        public FPNumber Magnitude => FPNumber.Sqrt(SqrMagnitude);

        public FPNumber SqrMagnitude => x * x + y * y + z * z;

        public FPVec3 Normalized => Magnitude > FPNumber.Zero ? this / Magnitude : Zero;

        public bool IsZero => x.IsZero && y.IsZero && z.IsZero;

        public FPVec2 Xy => new FPVec2(x, y);
        public FPVec2 Xz => new FPVec2(x, z);
        public FPVec2 Yz => new FPVec2(y, z);

        public FPNumber this[int index]
        {
            get
            {
                return index switch
                {
                    0 => x,
                    1 => y,
                    2 => z,
                    _ => throw new IndexOutOfRangeException("FPVec3 index out of range")
                };
            }
            set
            {
                switch (index)
                {
                    case 0: x = value; break;
                    case 1: y = value; break;
                    case 2: z = value; break;
                    default: throw new IndexOutOfRangeException("FPVec3 index out of range");
                }
            }
        }

        #endregion

        #region 基本运算

        public static FPVec3 operator +(FPVec3 a, FPVec3 b)
        {
            return new FPVec3(a.x + b.x, a.y + b.y, a.z + b.z);
        }

        public static FPVec3 operator -(FPVec3 a, FPVec3 b)
        {
            return new FPVec3(a.x - b.x, a.y - b.y, a.z - b.z);
        }

        public static FPVec3 operator -(FPVec3 a)
        {
            return new FPVec3(-a.x, -a.y, -a.z);
        }

        public static FPVec3 operator *(FPVec3 a, FPNumber b)
        {
            return new FPVec3(a.x * b, a.y * b, a.z * b);
        }

        public static FPVec3 operator *(FPNumber a, FPVec3 b)
        {
            return new FPVec3(a * b.x, a * b.y, a * b.z);
        }

        public static FPVec3 operator /(FPVec3 a, FPNumber b)
        {
            return new FPVec3(a.x / b, a.y / b, a.z / b);
        }

        public static bool operator ==(FPVec3 a, FPVec3 b)
        {
            return a.x == b.x && a.y == b.y && a.z == b.z;
        }

        public static bool operator !=(FPVec3 a, FPVec3 b)
        {
            return !(a == b);
        }

        #endregion

        #region 向量运算

        /// <summary>
        /// 点积
        /// </summary>
        public static FPNumber Dot(FPVec3 a, FPVec3 b)
        {
            return a.x * b.x + a.y * b.y + a.z * b.z;
        }

        /// <summary>
        /// 叉积
        /// </summary>
        public static FPVec3 Cross(FPVec3 a, FPVec3 b)
        {
            return new FPVec3(
                a.y * b.z - a.z * b.y,
                a.z * b.x - a.x * b.z,
                a.x * b.y - a.y * b.x
            );
        }

        /// <summary>
        /// 距离
        /// </summary>
        public static FPNumber Distance(FPVec3 a, FPVec3 b)
        {
            return (a - b).Magnitude;
        }

        /// <summary>
        /// 平方距离
        /// </summary>
        public static FPNumber SqrDistance(FPVec3 a, FPVec3 b)
        {
            return (a - b).SqrMagnitude;
        }

        /// <summary>
        /// 角度（弧度）
        /// </summary>
        public static FPNumber Angle(FPVec3 a, FPVec3 b)
        {
            FPNumber dot = Dot(a.Normalized, b.Normalized);
            dot = FPNumber.Max(-FPNumber.One, FPNumber.Min(FPNumber.One, dot));
            return FPNumber.Acos(dot);
        }

        /// <summary>
        /// 反射向量
        /// </summary>
        public static FPVec3 Reflect(FPVec3 inDirection, FPVec3 inNormal)
        {
            FPNumber dot = Dot(inDirection, inNormal);
            return inDirection - 2 * dot * inNormal;
        }

        /// <summary>
        /// 投影
        /// </summary>
        public static FPVec3 Project(FPVec3 vector, FPVec3 onNormal)
        {
            FPNumber sqrMag = onNormal.SqrMagnitude;
            if (sqrMag < FPNumber.FromFloat(1e-6f))
                return Zero;
            
            FPNumber dot = Dot(vector, onNormal);
            return onNormal * (dot / sqrMag);
        }

        /// <summary>
        /// 拒绝（垂直于投影）
        /// </summary>
        public static FPVec3 Reject(FPVec3 vector, FPVec3 onNormal)
        {
            return vector - Project(vector, onNormal);
        }

        /// <summary>
        /// 球面插值
        /// </summary>
        public static FPVec3 SphericalLerp(FPVec3 a, FPVec3 b, FPNumber t)
        {
            FPNumber angle = Angle(a, b);
            if (angle.IsZero)
                return a;

            FPNumber sinAngle = FPNumber.Sin(angle);
            FPNumber sinT = FPNumber.Sin(t * angle);
            FPNumber sin1T = FPNumber.Sin((FPNumber.One - t) * angle);

            return (a * sin1T + b * sinT) / sinAngle;
        }

        #endregion

        #region 插值函数

        /// <summary>
        /// 线性插值
        /// </summary>
        public static FPVec3 Lerp(FPVec3 a, FPVec3 b, FPNumber t)
        {
            return new FPVec3(
                FPNumber.Lerp(a.x, b.x, t),
                FPNumber.Lerp(a.y, b.y, t),
                FPNumber.Lerp(a.z, b.z, t)
            );
        }

        /// <summary>
        /// 平滑插值
        /// </summary>
        public static FPVec3 SmoothLerp(FPVec3 a, FPVec3 b, FPNumber t)
        {
            return new FPVec3(
                FPNumber.SmoothLerp(a.x, b.x, t),
                FPNumber.SmoothLerp(a.y, b.y, t),
                FPNumber.SmoothLerp(a.z, b.z, t)
            );
        }

        #endregion

        #region 实用函数

        /// <summary>
        /// 最小值
        /// </summary>
        public static FPVec3 Min(FPVec3 a, FPVec3 b)
        {
            return new FPVec3(
                FPNumber.Min(a.x, b.x),
                FPNumber.Min(a.y, b.y),
                FPNumber.Min(a.z, b.z)
            );
        }

        /// <summary>
        /// 最大值
        /// </summary>
        public static FPVec3 Max(FPVec3 a, FPVec3 b)
        {
            return new FPVec3(
                FPNumber.Max(a.x, b.x),
                FPNumber.Max(a.y, b.y),
                FPNumber.Max(a.z, b.z)
            );
        }

        /// <summary>
        /// 移动朝向目标
        /// </summary>
        public static FPVec3 MoveTowards(FPVec3 current, FPVec3 target, FPNumber maxDistanceDelta)
        {
            FPVec3 delta = target - current;
            FPNumber sqrDistance = delta.SqrMagnitude;

            if (sqrDistance <= maxDistanceDelta * maxDistanceDelta)
                return target;

            FPNumber distance = FPNumber.Sqrt(sqrDistance);
            return current + delta * (maxDistanceDelta / distance);
        }

        /// <summary>
        /// 旋转向量（绕Y轴）
        /// </summary>
        public static FPVec3 RotateY(FPVec3 vector, FPNumber angle)
        {
            FPNumber cos = FPNumber.Cos(angle);
            FPNumber sin = FPNumber.Sin(angle);
            return new FPVec3(
                vector.x * cos - vector.z * sin,
                vector.y,
                vector.x * sin + vector.z * cos
            );
        }

        /// <summary>
        /// 旋转向量（绕X轴）
        /// </summary>
        public static FPVec3 RotateX(FPVec3 vector, FPNumber angle)
        {
            FPNumber cos = FPNumber.Cos(angle);
            FPNumber sin = FPNumber.Sin(angle);
            return new FPVec3(
                vector.x,
                vector.y * cos - vector.z * sin,
                vector.y * sin + vector.z * cos
            );
        }

        /// <summary>
        /// 旋转向量（绕Z轴）
        /// </summary>
        public static FPVec3 RotateZ(FPVec3 vector, FPNumber angle)
        {
            FPNumber cos = FPNumber.Cos(angle);
            FPNumber sin = FPNumber.Sin(angle);
            return new FPVec3(
                vector.x * cos - vector.y * sin,
                vector.x * sin + vector.y * cos,
                vector.z
            );
        }

        /// <summary>
        /// 限制向量长度
        /// </summary>
        public FPVec3 ClampMagnitude(FPNumber maxLength)
        {
            FPNumber sqrMag = SqrMagnitude;
            if (sqrMag <= maxLength * maxLength)
                return this;

            FPNumber mag = FPNumber.Sqrt(sqrMag);
            return this * (maxLength / mag);
        }

        /// <summary>
        /// 设置向量长度
        /// </summary>
        public FPVec3 SetMagnitude(FPNumber magnitude)
        {
            return Normalized * magnitude;
        }

        /// <summary>
        /// 获取向量的欧拉角（弧度）
        /// </summary>
        public FPVec3 ToEulerAngles()
        {
            // 简化的欧拉角计算（假设ZYX顺序）
            FPNumber pitch = FPNumber.Asin(-z);
            FPNumber yaw = FPNumber.Atan2(x, y);
            FPNumber roll = FPNumber.Zero; // 简化处理

            return new FPVec3(pitch, yaw, roll);
        }

        /// <summary>
        /// 从欧拉角创建向量
        /// </summary>
        public static FPVec3 FromEulerAngles(FPVec3 eulerAngles)
        {
            FPNumber cosPitch = FPNumber.Cos(eulerAngles.x);
            FPNumber sinPitch = FPNumber.Sin(eulerAngles.x);
            FPNumber cosYaw = FPNumber.Cos(eulerAngles.y);
            FPNumber sinYaw = FPNumber.Sin(eulerAngles.y);

            return new FPVec3(
                cosPitch * sinYaw,
                cosPitch * cosYaw,
                -sinPitch
            );
        }

        /// <summary>
        /// 计算两个向量的叉积并归一化
        /// </summary>
        public static FPVec3 CrossNormalized(FPVec3 a, FPVec3 b)
        {
            return Cross(a, b).Normalized;
        }

        /// <summary>
        /// 计算三个点形成的三角形的法向量
        /// </summary>
        public static FPVec3 TriangleNormal(FPVec3 a, FPVec3 b, FPVec3 c)
        {
            FPVec3 edge1 = b - a;
            FPVec3 edge2 = c - a;
            return CrossNormalized(edge1, edge2);
        }

        /// <summary>
        /// 计算点到直线的距离
        /// </summary>
        public static FPNumber PointToLineDistance(FPVec3 point, FPVec3 lineStart, FPVec3 lineEnd)
        {
            FPVec3 lineDirection = (lineEnd - lineStart).Normalized;
            FPVec3 pointToStart = point - lineStart;
            FPVec3 projection = Project(pointToStart, lineDirection);
            return Distance(pointToStart, projection);
        }

        /// <summary>
        /// 计算点到平面的距离
        /// </summary>
        public static FPNumber PointToPlaneDistance(FPVec3 point, FPVec3 planePoint, FPVec3 planeNormal)
        {
            FPVec3 pointToPlane = point - planePoint;
            return Dot(pointToPlane, planeNormal);
        }

        #endregion

        #region 类型转换

        public static implicit operator FPVec3(Vector3 vector) => new FPVec3(vector);
        public static explicit operator Vector3(FPVec3 vector) => new Vector3(vector.x.AsFloat, vector.y.AsFloat, vector.z.AsFloat);

        #endregion

        #region IEquatable 实现

        public bool Equals(FPVec3 other)
        {
            return x == other.x && y == other.y && z == other.z;
        }

        public override bool Equals(object obj)
        {
            return obj is FPVec3 other && Equals(other);
        }

        public override int GetHashCode()
        {
            return HashCode.Combine(x, y, z);
        }

        #endregion

        #region ToString

        public override string ToString()
        {
            return $"({x}, {y}, {z})";
        }

        public string ToString(string format)
        {
            return $"({x.ToString(format)}, {y.ToString(format)}, {z.ToString(format)})";
        }

        #endregion
    }
} 